/**
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  * @file       gimbal_task.c/h
  * @brief      2025赛季哨兵云台任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2018     RM              1. 完成
  *  V1.1.0     Nov-11-2019     RM              1. 添加一些注释
  *  V2.0.0     Jan-1-2024      申欣宇           1. 优化了手动操作下云台的控制以及上电校准
  *  V3.0.0     Dec-20-2024     吴森桦           1. 添加自瞄功能和巡逻功能
  *                             冉文治           2. 添加大yaw辅助雷达进行导航
  *                                             3. 删去多余的功能
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2025 ROBOT-Z****************************
  */

#include "gimbal_task.h"
#include "main.h"
#include "cmsis_os.h"
#include "chassis_task.h"
#include "arm_math.h"
#include "CAN_receive.h"
#include "user_lib.h"
#include "detect_task.h"
#include "remote_control.h"
#include "gimbal_behaviour.h"
#include "INS_task.h"
#include "shoot.h"
#include "pid.h"
#include "usb_task.h"
#include "bsp_dwt.h"
#include "referee.h"

// 电机编码值规整 0—8191
#define ecd_format(ecd)         \
    {                           \
        if ((ecd) > ECD_RANGE)  \
            (ecd) -= ECD_RANGE; \
        else if ((ecd) < 0)     \
            (ecd) += ECD_RANGE; \
    }

#define gimbal_total_pid_clear(gimbal_clear)                                                    \
    {                                                                                           \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_absolute_angle_pid);    \
        PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_absolute_gyro_speed_pid);      \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_relative_angle_pid);    \
        PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_relative_enconde_speed_pid);   \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_CV_angle_pid);          \
        PID_clear(&(gimbal_clear)->gimbal_yaw_motor.gimbal_motor_CV_gyro_speed_pid);            \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_absolute_angle_pid);  \
        PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_absolute_gyro_speed_pid);    \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_relative_angle_pid);  \
        PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_relative_enconde_speed_pid); \
        gimbal_PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_CV_angle_pid);        \
        PID_clear(&(gimbal_clear)->gimbal_pitch_motor.gimbal_motor_CV_gyro_speed_pid);          \
    }

#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t gimbal_high_water;
#endif

/**
  * @brief          初始化"gimbal_control"变量,包括pid初始化,遥控器指针初始化,云台电机指针初始化,陀螺仪角度指针初始化
  * @author         吴森桦
  * @param          gimbal_init:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_init(gimbal_control_t *gimbal_init);

/**
  * @brief          设置云台控制模式,主要在'gimbal_behaviour_mode_set'函数中改变
  * @author         吴森桦
  * @param          gimbal_set_mode "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_mode(gimbal_control_t *gimbal_set_mode);

/**
  * @brief          底盘测量数据更新,包括电机速度,欧拉角度,机器人速度
  * @author         吴森桦
  * @param          gimbal_feedback_update "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_feedback_update(gimbal_control_t *gimbal_feedback_update);

/**
  * @brief          云台模式改变,有些参数需要改变,例如控制yaw角度设定值应该变成当前yaw角度
  * @author         吴森桦
  * @param          gimbal_mode_change "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_mode_change_control_transit(gimbal_control_t *gimbal_mode_change);

/**
  * @brief          计算ecd与offset_ecd之间的相对角度
  * @author         吴森桦
  * @param          ecd 电机当前编码
  * @param          offset_ecd 电机中值编码
  * @retval         相对角度，单位rad
  */
static fp32 motor_ecd_to_angle_change(uint16_t ecd, uint16_t offset_ecd);

/**
  * @brief          设置云台控制设定值,控制值是通过gimbal_behaviour_control_set函数设置的
  * @author         吴森桦
  * @param          gimbal_set_control "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_control(gimbal_control_t *gimbal_set_control);

/**
  * @brief          控制循环，根据控制设定值，计算电机电流值，进行控制
  * @author         吴森桦
  * @param          gimbal_control_loop:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_control_loop(gimbal_control_t *gimbal_control_loop);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @retval         none
  */
static void gimbal_motor_absolute_yaw_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @retval         none
  */
static void gimbal_motor_absolute_pitch_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @retval         none
  */
static void gimbal_motor_relative_yaw_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @retval         none
  */
static void gimbal_motor_relative_pitch_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_CV，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @retval         none
  */
static void gimbal_motor_CV_yaw_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_CV，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @retval         none
  */
static void gimbal_motor_CV_pitch_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_RAW，电流值直接发送到CAN总线
  * @author         吴森桦
  * @param          gimbal_motor yaw电机或者pitch电机
  * @retval         none
  */
static void gimbal_motor_raw_angle_control(gimbal_motor_t *gimbal_motor);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO,使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_control yaw电机
  * @param          yaw_add 角度增加量
  * @retval         none
  */
static void gimbal_absolute_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add);

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO,使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_control pitch电机
  * @param          pitch_add 角度增加量
  * @retval         none
  */
static void gimbal_absolute_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add);

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @param          yaw_add 角度增加量
  * @retval         none
  */
static void gimbal_relative_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add);

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @param          pitch_add 角度增加量
  * @retval         none
  */
static void gimbal_relative_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add);

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_CV，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_control yaw电机
  * @param          yaw_add 角度增加量
  * @retval         none
  */
static void gimbal_cv_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add);

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_CV，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_control pitch电机
  * @param          pitch_add 角度增加量
  * @retval         none
  */
static void gimbal_cv_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add);

/**
  * @brief          云台角度PID初始化, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:云台PID指针
  * @param[in]      maxout: pid最大输出
  * @param[in]      intergral_limit: pid最大积分输出
  * @param[in]      kp: pid kp
  * @param[in]      ki: pid ki
  * @param[in]      kd: pid kd
  * @retval         none
  */
static void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 intergral_limit, fp32 kp, fp32 ki, fp32 kd);

/**
  * @brief          云台角度PID计算, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:云台PID指针
  * @param[in]      get: 角度反馈
  * @param[in]      set: 角度设定
  * @param[in]      error_delta: 角速度
  * @retval         pid 输出
  */
static fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta);

/**
  * @brief          云台PID清除，清除pid的out,iout
  * @param[out]     pid_clear:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_PID_clear(gimbal_PID_t *pid_clear);

// 云台控制所有相关数据
gimbal_control_t gimbal_control;

/**
  * @brief          云台任务，间隔 GIMBAL_CONTROL_TIME 1ms
  * @author         吴森桦
  * @param          pvParameters 空
  * @retval         none
  */
void gimbal_task(void const *pvParameters)
{
    // 等待陀螺仪任务更新陀螺仪数据
    vTaskDelay(GIMBAL_TASK_INIT_TIME);
    // 云台初始化
    gimbal_init(&gimbal_control);
    // 射击初始化
    shoot_init();
    // 判断电机是否都上线
    while (toe_is_error(YAW_GIMBAL_MOTOR_TOE) || toe_is_error(PITCH_GIMBAL_MOTOR_TOE))
    {
        vTaskDelay(GIMBAL_CONTROL_TIME);
        // 云台数据反馈
        gimbal_feedback_update(&gimbal_control);
    }

    while (1)
    {
        // 设置云台控制模式
        gimbal_set_mode(&gimbal_control);
        // 控制模式切换 控制数据过渡
        gimbal_mode_change_control_transit(&gimbal_control);
        // 云台数据反馈
        gimbal_feedback_update(&gimbal_control);
        // 设置云台控制量
        gimbal_set_control(&gimbal_control);
        // 云台控制PID计算
        gimbal_control_loop(&gimbal_control);
        // 射击任务控制循环
        shoot_control_loop();

        // 确保至少一个电机在线
        if (!(toe_is_error(YAW_GIMBAL_MOTOR_TOE) && toe_is_error(PITCH_GIMBAL_MOTOR_TOE)))
        {
            // 当遥控器掉线的时候，发送给云台电机电流为0
            if (toe_is_error(DBUS_TOE) && game_state_t.game_progress != 4)
            {
                CAN_cmd_gimbal(0, 0, 0, 0);
                CAN_cmd_shoot(0, 0, 0, 0);
            }
            else
            {
                // 发送控制电流
                CAN_cmd_gimbal(gimbal_control.gimbal_yaw_motor.given_current, -gimbal_control.gimbal_pitch_motor.given_current, gimbal_control.shoot_control_measure->trigger_motor.trigger_given_current, 0);
                CAN_cmd_shoot(gimbal_control.shoot_control_measure->fric_motor[0].fric_given_current, gimbal_control.shoot_control_measure->fric_motor[1].fric_given_current, 0, 0);
            }
        }

        vTaskDelay(GIMBAL_CONTROL_TIME);
#if INCLUDE_uxTaskGetStackHighWaterMark
        gimbal_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}

/**
  * @brief          返回yaw 电机数据指针
  * @author         吴森桦
  * @param          none
  * @retval         yaw电机指针
  */
const gimbal_motor_t *get_yaw_motor_point(void)
{
    return &gimbal_control.gimbal_yaw_motor;
}

/**
  * @brief          返回pitch 电机数据指针
  * @author         吴森桦
  * @param          none
  * @retval         pitch电机指针
  */
const gimbal_motor_t *get_pitch_motor_point(void)
{
    return &gimbal_control.gimbal_pitch_motor;
}

/**
  * @brief          初始化"gimbal_control"变量,包括pid初始化,遥控器指针初始化,云台电机指针初始化,陀螺仪角度指针初始化
  * @author         吴森桦
  * @param          gimbal_init:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_init(gimbal_control_t *gimbal_init)
{
    // yaw和pitch中值
    gimbal_init->gimbal_yaw_motor.offset_ecd = 4096;
    gimbal_init->gimbal_yaw_motor.max_relative_angle = 2.5f;
    gimbal_init->gimbal_yaw_motor.min_relative_angle = -2.5f;
    gimbal_init->gimbal_yaw_motor.max_absolute_angle = PI;
    gimbal_init->gimbal_yaw_motor.min_absolute_angle = -PI;

    gimbal_init->gimbal_pitch_motor.offset_ecd = 1713;
    gimbal_init->gimbal_pitch_motor.max_relative_angle = 0.49f;
    gimbal_init->gimbal_pitch_motor.min_relative_angle = -0.43f;
    gimbal_init->gimbal_pitch_motor.max_absolute_angle = 0.49f;
    gimbal_init->gimbal_pitch_motor.min_absolute_angle = -0.43f;

    // 绝对角度下(陀螺仪),pitch轴电机和小yaw轴电机PID速度环
    static const fp32 Pitch_gyro_speed_pid[3] = {PITCH_GYRO_SPEED_PID_KP, PITCH_GYRO_SPEED_PID_KI, PITCH_GYRO_SPEED_PID_KD};
    static const fp32 Yaw_gyro_speed_pid[3] = {YAW_GYRO_SPEED_PID_KP, YAW_GYRO_SPEED_PID_KI, YAW_GYRO_SPEED_PID_KD};
		
    static const fp32 Pitch_enconde_speed_pid[3] = {PITCH_ENCONDE_SPEED_PID_KP, PITCH_ENCONDE_SPEED_PID_KI, PITCH_ENCONDE_SPEED_PID_KD};
    static const fp32 Yaw_enconde_speed_pid[3] = {YAW_ENCONDE_SPEED_PID_KP, YAW_ENCONDE_SPEED_PID_KI, YAW_ENCONDE_SPEED_PID_KD};

    static const fp32 Pitch_cv_gyro_speed_pid[3] = {PITCH_CV_SPEED_PID_KP, PITCH_CV_SPEED_PID_KI, PITCH_CV_SPEED_PID_KD};
    static const fp32 Yaw_cv_gyro_speed_pid[3] = {YAW_CV_SPEED_PID_KP, YAW_CV_SPEED_PID_KI, YAW_CV_SPEED_PID_KD};

    // 电机数据指针获取
    gimbal_init->gimbal_yaw_motor.gimbal_motor_measure = get_yaw_gimbal_motor_measure_point();
    gimbal_init->gimbal_pitch_motor.gimbal_motor_measure = get_pitch_gimbal_motor_measure_point();
    // 陀螺仪数据指针获取
    gimbal_init->gimbal_INT_angle_point = get_INS_angle_point();
    gimbal_init->gimbal_INT_gyro_point = get_gyro_data_point();
    // 遥控器数据指针获取
    gimbal_init->gimbal_rc_ctrl = get_remote_control_point();
    //视觉指针获取
    gimbal_init->cv_ctrl = get_cv_point();
    // 获得发射机构数据指针
    gimbal_init->shoot_control_measure = get_shoot_motor_point();
    // 初始化电机模式
    gimbal_init->gimbal_yaw_motor.gimbal_motor_mode = gimbal_init->gimbal_yaw_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
    gimbal_init->gimbal_pitch_motor.gimbal_motor_mode = gimbal_init->gimbal_pitch_motor.last_gimbal_motor_mode = GIMBAL_MOTOR_RAW;
    // 初始化小yaw轴电机pid
    gimbal_PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_absolute_angle_pid, YAW_ABSOLUTE_ANGLE_PID_MAX_OUT, YAW_ABSOLUTE_ANGLE_PID_MAX_IOUT, YAW_ABSOLUTE_ANGLE_PID_KP, YAW_ABSOLUTE_ANGLE_PID_KI, YAW_ABSOLUTE_ANGLE_PID_KD);
    gimbal_PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_relative_angle_pid, YAW_RELATIVE_ANGLE_PID_MAX_OUT, YAW_RELATIVE_ANGLE_PID_MAX_IOUT, YAW_RELATIVE_ANGLE_PID_KP, YAW_RELATIVE_ANGLE_PID_KI, YAW_RELATIVE_ANGLE_PID_KD);
    gimbal_PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_CV_angle_pid, YAW_CV_ANGLE_PID_MAX_OUT, YAW_CV_ANGLE_PID_MAX_IOUT, YAW_CV_ANGLE_PID_KP, YAW_CV_ANGLE_PID_KI, YAW_CV_ANGLE_PID_KD);
    PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_absolute_gyro_speed_pid, PID_POSITION, Yaw_gyro_speed_pid, YAW_GYRO_SPEED_PID_MAX_OUT, YAW_GYRO_SPEED_PID_MAX_IOUT);
    PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_relative_enconde_speed_pid, PID_POSITION, Yaw_enconde_speed_pid, YAW_ENCONDE_SPEED_PID_MAX_OUT, YAW_ENCONDE_SPEED_PID_MAX_IOUT);
    PID_init(&gimbal_init->gimbal_yaw_motor.gimbal_motor_CV_gyro_speed_pid, PID_POSITION, Yaw_cv_gyro_speed_pid, YAW_CV_SPEED_PID_MAX_OUT, YAW_CV_SPEED_PID_MAX_IOUT);

    //初始化pitch轴电机pid
    gimbal_PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_absolute_angle_pid, PITCH_ABSOLUTE_ANGLE_PID_MAX_OUT, PITCH_ABSOLUTE_ANGLE_PID_MAX_IOUT, PITCH_ABSOLUTE_ANGLE_PID_KP, PITCH_ABSOLUTE_ANGLE_PID_KI, PITCH_ABSOLUTE_ANGLE_PID_KD);
    gimbal_PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_relative_angle_pid, PITCH_RELATIVE_ANGLE_PID_MAX_OUT, PITCH_RELATIVE_ANGLE_PID_MAX_IOUT, PITCH_RELATIVE_ANGLE_PID_KP, PITCH_RELATIVE_ANGLE_PID_KI, PITCH_RELATIVE_ANGLE_PID_KD);
    gimbal_PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_CV_angle_pid, PITCH_CV_ANGLE_PID_MAX_OUT, PITCH_CV_ANGLE_PID_MAX_IOUT, PITCH_CV_ANGLE_PID_KP, PITCH_CV_ANGLE_PID_KI, PITCH_CV_ANGLE_PID_KD);
    PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_absolute_gyro_speed_pid, PID_POSITION, Pitch_gyro_speed_pid, PITCH_GYRO_SPEED_PID_MAX_OUT, PITCH_GYRO_SPEED_PID_MAX_IOUT);
    PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_relative_enconde_speed_pid, PID_POSITION, Pitch_enconde_speed_pid, PITCH_ENCONDE_SPEED_PID_MAX_OUT, PITCH_ENCONDE_SPEED_PID_MAX_IOUT);
    PID_init(&gimbal_init->gimbal_pitch_motor.gimbal_motor_CV_gyro_speed_pid, PID_POSITION, Pitch_cv_gyro_speed_pid, PITCH_CV_SPEED_PID_MAX_OUT, PITCH_CV_SPEED_PID_MAX_IOUT);
    // 清除所有PID
    gimbal_total_pid_clear(gimbal_init);
    // 更新下一次数据
    gimbal_feedback_update(gimbal_init);

    // 初始化一下特定参数
    gimbal_init->gimbal_yaw_motor.absolute_angle_set = gimbal_init->gimbal_yaw_motor.absolute_angle;
    gimbal_init->gimbal_yaw_motor.motor_gyro_set = gimbal_init->gimbal_yaw_motor.motor_gyro;
    gimbal_init->gimbal_yaw_motor.relative_angle_set = gimbal_init->gimbal_yaw_motor.relative_angle;
    gimbal_init->gimbal_yaw_motor.motor_enconde_speed_set = gimbal_init->gimbal_yaw_motor.motor_enconde_speed;

    gimbal_init->gimbal_pitch_motor.absolute_angle_set = gimbal_init->gimbal_pitch_motor.absolute_angle;
    gimbal_init->gimbal_pitch_motor.relative_angle_set = gimbal_init->gimbal_pitch_motor.relative_angle;
    gimbal_init->gimbal_pitch_motor.motor_gyro_set = gimbal_init->gimbal_pitch_motor.motor_gyro;
    gimbal_init->gimbal_pitch_motor.motor_enconde_speed_set = gimbal_init->gimbal_pitch_motor.motor_enconde_speed;
}

/**
  * @brief          设置云台控制模式,主要在'gimbal_behaviour_mode_set'函数中改变
  * @author         吴森桦
  * @param          gimbal_set_mode "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_mode(gimbal_control_t *gimbal_set_mode)
{
    if (gimbal_set_mode == NULL)
    {
        return;
    }

    gimbal_behaviour_mode_set(gimbal_set_mode);
}

/**
  * @brief          底盘测量数据更新,包括电机速度,欧拉角度,机器人速度
  * @author         吴森桦
  * @param          gimbal_feedback_update "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_feedback_update(gimbal_control_t *gimbal_feedback_update)
{
    if (gimbal_feedback_update == NULL)
    {
        return;
    }

    // 更新dwt定时器
    gimbal_feedback_update->dt = DWT_GetDeltaT(&gimbal_feedback_update->gimbal_dwt_count);

    if (toe_is_error(YAW_GIMBAL_MOTOR_TOE) && toe_is_error(PITCH_GIMBAL_MOTOR_TOE))
    {
        chassis_move.chassis_state = CHASSIS_OFFLINE;
    }
		else if (!(toe_is_error(YAW_GIMBAL_MOTOR_TOE) && toe_is_error(PITCH_GIMBAL_MOTOR_TOE)))
		{
			if (chassis_move.chassis_state == CHASSIS_OFFLINE)
			{
			    static uint16_t i = 0;
			    i++;
			    if (i >= 1000)
			    {
			        chassis_move.chassis_state = CHASSIS_ONLINE;
					i = 0;
			    }
			}
		}

    // 云台数据更新
    gimbal_feedback_update->gimbal_pitch_motor.absolute_angle = *(gimbal_feedback_update->gimbal_INT_angle_point + INS_PITCH_ADDRESS_OFFSET);
    gimbal_feedback_update->gimbal_pitch_motor.relative_angle = -motor_ecd_to_angle_change(gimbal_feedback_update->gimbal_pitch_motor.gimbal_motor_measure->ecd, gimbal_feedback_update->gimbal_pitch_motor.offset_ecd);
    gimbal_feedback_update->gimbal_pitch_motor.motor_gyro = *(gimbal_feedback_update->gimbal_INT_gyro_point + INS_GYRO_Y_ADDRESS_OFFSET);
    gimbal_feedback_update->gimbal_pitch_motor.motor_enconde_speed = gimbal_feedback_update->gimbal_pitch_motor.gimbal_motor_measure->speed_rpm;
    gimbal_feedback_update->gimbal_yaw_motor.absolute_angle = *(gimbal_feedback_update->gimbal_INT_angle_point + INS_YAW_ADDRESS_OFFSET);
    gimbal_feedback_update->gimbal_yaw_motor.relative_angle = motor_ecd_to_angle_change(gimbal_feedback_update->gimbal_yaw_motor.gimbal_motor_measure->ecd, gimbal_feedback_update->gimbal_yaw_motor.offset_ecd);
    gimbal_feedback_update->cv_ctrl->dif_yaw = gimbal_feedback_update->gimbal_yaw_motor.relative_angle;
    gimbal_feedback_update->gimbal_yaw_motor.motor_gyro = arm_cos_f32(gimbal_feedback_update->gimbal_pitch_motor.relative_angle) * (*(gimbal_feedback_update->gimbal_INT_gyro_point + INS_GYRO_Z_ADDRESS_OFFSET)) - arm_sin_f32(gimbal_feedback_update->gimbal_pitch_motor.relative_angle) * (*(gimbal_feedback_update->gimbal_INT_gyro_point + INS_GYRO_X_ADDRESS_OFFSET));
    gimbal_feedback_update->gimbal_yaw_motor.motor_enconde_speed = gimbal_feedback_update->gimbal_yaw_motor.gimbal_motor_measure->speed_rpm;

    if(gimbal_feedback_update->gimbal_yaw_motor.relative_angle > PI / 2)
        gimbal_feedback_update->gimbal_yaw_patrol_speed = YAW_PATROL;
    else if(gimbal_feedback_update->gimbal_yaw_motor.relative_angle < -PI / 2)
        gimbal_feedback_update->gimbal_yaw_patrol_speed = -YAW_PATROL;
}

/**
  * @brief          计算ecd与offset_ecd之间的相对角度
  * @author         吴森桦
  * @param          ecd 电机当前编码
  * @param          offset_ecd 电机中值编码
  * @retval         相对角度，单位rad
  */
static fp32 motor_ecd_to_angle_change(uint16_t ecd, uint16_t offset_ecd)
{
    int32_t relative_ecd = ecd - offset_ecd;
    if (relative_ecd > HALF_ECD_RANGE)
    {
        relative_ecd -= ECD_RANGE;
    }
    else if (relative_ecd < -HALF_ECD_RANGE)
    {
        relative_ecd += ECD_RANGE;
    }

    return relative_ecd * MOTOR_ECD_TO_RAD;
}

/**
  * @brief          云台模式改变,有些参数需要改变,例如控制yaw角度设定值应该变成当前yaw角度
  * @author         吴森桦
  * @param          gimbal_mode_change "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_mode_change_control_transit(gimbal_control_t *gimbal_mode_change)
{
    if (gimbal_mode_change == NULL)
    {
        return;
    }

    // yaw电机状态机切换保存数据
    if (gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode != gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode)
    {
        if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_RAW && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
        {
            gimbal_mode_change->gimbal_yaw_motor.raw_cmd_current = gimbal_mode_change->gimbal_yaw_motor.current_set = gimbal_mode_change->gimbal_yaw_motor.given_current;
        }
        else if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_GYRO && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
        {
            gimbal_mode_change->gimbal_yaw_motor.absolute_angle_set = gimbal_mode_change->gimbal_yaw_motor.absolute_angle;
        }
        else if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_ENCONDE && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
        {
            gimbal_mode_change->gimbal_yaw_motor.relative_angle_set = gimbal_mode_change->gimbal_yaw_motor.relative_angle;
        }
        else if (gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_CV && gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
        {
            gimbal_mode_change->gimbal_yaw_motor.absolute_angle_set = gimbal_mode_change->gimbal_yaw_motor.absolute_angle;
        }
    }
    gimbal_mode_change->gimbal_yaw_motor.last_gimbal_motor_mode = gimbal_mode_change->gimbal_yaw_motor.gimbal_motor_mode;

    // pitch电机状态机切换保存数据
    if (gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode != gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode)
    {
        if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_RAW && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
        {
            gimbal_mode_change->gimbal_pitch_motor.raw_cmd_current = gimbal_mode_change->gimbal_pitch_motor.current_set = gimbal_mode_change->gimbal_pitch_motor.given_current;
        }
        else if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_GYRO && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
        {
            gimbal_mode_change->gimbal_pitch_motor.absolute_angle_set = gimbal_mode_change->gimbal_pitch_motor.absolute_angle;
        }
        else if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_ENCONDE && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
        {
            gimbal_mode_change->gimbal_pitch_motor.relative_angle_set = gimbal_mode_change->gimbal_pitch_motor.relative_angle;
        }
        else if (gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode != GIMBAL_MOTOR_CV && gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
        {
            gimbal_mode_change->gimbal_pitch_motor.relative_angle_set = gimbal_mode_change->gimbal_pitch_motor.relative_angle;
        }
    }
    gimbal_mode_change->gimbal_pitch_motor.last_gimbal_motor_mode = gimbal_mode_change->gimbal_pitch_motor.gimbal_motor_mode;
}

/**
  * @brief          设置云台控制设定值,控制值是通过gimbal_behaviour_control_set函数设置的
  * @author         吴森桦
  * @param          gimbal_set_control "gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_set_control(gimbal_control_t *gimbal_set_control)
{
    if (gimbal_set_control == NULL)
    {
        return;
    }
    fp32 add_yaw_angle = 0.0f;
    fp32 add_pitch_angle = 0.0f;

    gimbal_behaviour_control_set(&add_yaw_angle, &add_pitch_angle, gimbal_set_control);

    // yaw电机模式控制
    if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        // raw模式下,直接发送控制值
        gimbal_set_control->gimbal_yaw_motor.raw_cmd_current = add_yaw_angle;
    }
    else if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
    {
        // gyro模式下,陀螺仪角度控制
        gimbal_absolute_yaw_angle_limit(gimbal_set_control, add_yaw_angle);
    }
    else if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        // enconde模式下,电机编码角度控制
        if (gimbal_set_control->gimbal_yaw_stop_time)
        {
            gimbal_set_control->gimbal_yaw_stop_time += gimbal_set_control->dt;
            gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode = GIMBAL_MOTOR_GYRO;
            add_yaw_angle = 0.0f;
            gimbal_absolute_yaw_angle_limit(gimbal_set_control, add_yaw_angle);
            if (gimbal_set_control->gimbal_yaw_stop_time >= 0.25f)
            {
                gimbal_set_control->gimbal_yaw_stop_time = 0.0f;
                gimbal_set_control->gimbal_yaw_motor.relative_angle_set = gimbal_set_control->gimbal_yaw_motor.relative_angle;
                PID_change(&gimbal_set_control->gimbal_yaw_motor.gimbal_motor_absolute_gyro_speed_pid, YAW_GYRO_SPEED_PID_KP, YAW_GYRO_SPEED_PID_KI, YAW_GYRO_SPEED_PID_KD, YAW_GYRO_SPEED_PID_MAX_OUT, YAW_GYRO_SPEED_PID_MAX_IOUT);
            }
        }
        else
        {
            gimbal_set_control->gimnal_yaw_run_time += gimbal_set_control->dt;
            gimbal_relative_yaw_angle_limit(gimbal_set_control, add_yaw_angle);
            if (gimbal_set_control->gimnal_yaw_run_time >= 0.2f)
            {
                gimbal_set_control->gimnal_yaw_run_time = 0.0f;
                gimbal_set_control->gimbal_yaw_stop_time += gimbal_set_control->dt;
                gimbal_set_control->gimbal_yaw_motor.absolute_angle_set = gimbal_set_control->gimbal_yaw_motor.absolute_angle;
                PID_change(&gimbal_set_control->gimbal_yaw_motor.gimbal_motor_absolute_gyro_speed_pid, YAW_PATROL_GYRO_SPEED_PID_KP, YAW_PATROL_GYRO_SPEED_PID_KI, YAW_PATROL_GYRO_SPEED_PID_KD, YAW_PATROL_GYRO_SPEED_PID_MAX_OUT, YAW_PATROL_GYRO_SPEED_PID_MAX_IOUT);
            }
        }
    }
    else if (gimbal_set_control->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
    {
        // CV???????λ????
        gimbal_cv_yaw_angle_limit(gimbal_set_control, add_yaw_angle);
    }

    // pitch电机控制模式
    if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        // raw模式下,直接发送控制值
        gimbal_set_control->gimbal_pitch_motor.raw_cmd_current = add_pitch_angle;
    }
    else if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
    {
        // gyro模式下,陀螺仪角度控制
        gimbal_absolute_pitch_angle_limit(gimbal_set_control, add_pitch_angle);
    }
    else if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        // enconde模式下,电机编码角度控制
        gimbal_relative_pitch_angle_limit(gimbal_set_control, add_pitch_angle);
    }
    else if (gimbal_set_control->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
    {
        // CV???????λ????
        gimbal_cv_pitch_angle_limit(gimbal_set_control, add_pitch_angle);
    }
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO,使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_control yaw电机
  * @param          yaw_add 角度增加量
  * @retval         none
  */
static void gimbal_absolute_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add)
{
    static fp32 yaw_angle_set;
    if (gimbal_control == NULL)
    {
        return;
    }

    yaw_angle_set = gimbal_control->gimbal_yaw_motor.absolute_angle_set;
    gimbal_control->gimbal_yaw_motor.absolute_angle_set = rad_format(yaw_angle_set + yaw_add);
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO,使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_control pitch电机
  * @param          pitch_add 角度增加量
  * @retval         none
  */
static void gimbal_absolute_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add)
{
    static fp32 bias_angle;
    static fp32 pitch_angle_set;
    if (gimbal_control == NULL)
    {
        return;
    }

    // 当前控制误差角度
    bias_angle = rad_format(gimbal_control->gimbal_pitch_motor.absolute_angle_set - gimbal_control->gimbal_pitch_motor.absolute_angle);
    // 云台相对角度+误差角度+新增角度 如果大于最大机械角度
    if (gimbal_control->gimbal_pitch_motor.absolute_angle + bias_angle + pitch_add > gimbal_control->gimbal_pitch_motor.max_absolute_angle)
    {
        // 如果是往最大机械角度控制方向
        if (pitch_add > 0.0f)
        {
            // 计算出一个最大的添加角度
            pitch_add = gimbal_control->gimbal_pitch_motor.max_absolute_angle - gimbal_control->gimbal_pitch_motor.absolute_angle - bias_angle;
        }
    }
    // 云台相对角度+误差角度+新增角度 如果小于最小机械角度
    else if (gimbal_control->gimbal_pitch_motor.absolute_angle + bias_angle + pitch_add < gimbal_control->gimbal_pitch_motor.min_absolute_angle)
    {
        // 如果是往最小的添加角度
        if (pitch_add < 0.0f)
        {
            // 计算出一个最小的添加角度
            pitch_add = gimbal_control->gimbal_pitch_motor.min_absolute_angle - gimbal_control->gimbal_pitch_motor.absolute_angle - bias_angle;
        }
    }
    pitch_angle_set = gimbal_control->gimbal_pitch_motor.absolute_angle_set;
    gimbal_control->gimbal_pitch_motor.absolute_angle_set = rad_format(pitch_angle_set + pitch_add);
}

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @param          yaw_add 角度增加量
  * @retval         none
  */
static void gimbal_relative_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add)
{
	static fp32 yaw_angle_set;
	
    if (gimbal_control == NULL)
    {
        return;
    }

    yaw_angle_set = gimbal_control->gimbal_yaw_motor.relative_angle_set;
    gimbal_control->gimbal_yaw_motor.relative_angle_set = rad_format(yaw_angle_set + yaw_add);
}

/**
  * @brief          云台控制模式:GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @param          pitch_add 角度增加量
  * @retval         none
  */
static void gimbal_relative_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add)
{
    static fp32 bias_angle;
    static fp32 pitch_angle_set;
    if (gimbal_control == NULL)
    {
        return;
    }

    // 当前控制误差角度
    bias_angle = rad_format(gimbal_control->gimbal_pitch_motor.relative_angle_set - gimbal_control->gimbal_pitch_motor.relative_angle);
    // 云台相对角度+误差角度+新增角度 如果大于最大机械角度
    if (gimbal_control->gimbal_pitch_motor.relative_angle + bias_angle + pitch_add > gimbal_control->gimbal_pitch_motor.max_relative_angle)
    {
        // 如果是往最大机械角度控制方向
        if (pitch_add > 0.0f)
        {
            // 计算出一个最大的添加角度
            pitch_add = gimbal_control->gimbal_pitch_motor.max_relative_angle - gimbal_control->gimbal_pitch_motor.relative_angle - bias_angle;
        }
    }
    // 云台相对角度+误差角度+新增角度 如果小于最小机械角度
    else if (gimbal_control->gimbal_pitch_motor.relative_angle + bias_angle + pitch_add < gimbal_control->gimbal_pitch_motor.min_relative_angle)
    {
        // 如果是往最小的添加角度
        if (pitch_add < 0.0f)
        {
            // 计算出一个最小的添加角度
            pitch_add = gimbal_control->gimbal_pitch_motor.min_relative_angle - gimbal_control->gimbal_pitch_motor.relative_angle - bias_angle;
        }
    }
    pitch_angle_set = gimbal_control->gimbal_pitch_motor.relative_angle_set;
    gimbal_control->gimbal_pitch_motor.relative_angle_set = rad_format(pitch_angle_set + pitch_add);
}

static void gimbal_cv_yaw_angle_limit(gimbal_control_t *gimbal_control, fp32 yaw_add) // add????
{                                                                                 /* gimbal_cv_angle_limit gimbal_absolute_angle_limit  */
    if (gimbal_control == NULL)
    {
        return;
    }
    if(gimbal_control->cv_ctrl->target == 1)//&& (fabs(gimbal_control->gimbal_yaw_motor.absolute_angle_set-gimbal_control->gimbal_yaw_motor.absolute_angle) < 0.01))
    {
        gimbal_control->gimbal_yaw_motor.absolute_angle_set = yaw_add;//?????????
        //gimbal_control->cv_ctrl->Receice_yaw_Flag = 0;
    }
}

static void gimbal_cv_pitch_angle_limit(gimbal_control_t *gimbal_control, fp32 pitch_add) // add????
{                                                                                   /* gimbal_cv_angle_limit gimbal_absolute_angle_limit  */
    if (gimbal_control == NULL)
    {
        return;
    }
    if(gimbal_control->cv_ctrl->target == 1)//&& (fabs(gimbal_control->gimbal_yaw_motor.absolute_angle_set-gimbal_control->gimbal_yaw_motor.absolute_angle) < 0.01))
    {

        gimbal_control->gimbal_pitch_motor.relative_angle_set =  pitch_add;
        //gimbal_control->cv_ctrl->Receice_pitch_Flag = 0;
    }
}

/**
  * @brief          控制循环，根据控制设定值，计算电机电流值，进行控制
  * @author         吴森桦
  * @param          gimbal_control_loop:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_control_loop(gimbal_control_t *gimbal_control_loop)
{
    if (gimbal_control_loop == NULL)
    {
        return;
    }

    if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        gimbal_motor_raw_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
    }
    else if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
    {
        gimbal_motor_absolute_yaw_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
    }
    else if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        gimbal_motor_relative_yaw_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
    }
    else if (gimbal_control_loop->gimbal_yaw_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
    {
        gimbal_motor_CV_yaw_angle_control(&gimbal_control_loop->gimbal_yaw_motor);
    }

    if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_RAW)
    {
        gimbal_motor_raw_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
    else if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_GYRO)
    {
        gimbal_motor_absolute_pitch_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
    else if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_ENCONDE)
    {
        gimbal_motor_relative_pitch_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
    else if (gimbal_control_loop->gimbal_pitch_motor.gimbal_motor_mode == GIMBAL_MOTOR_CV)
    {
        gimbal_motor_CV_pitch_angle_control(&gimbal_control_loop->gimbal_pitch_motor);
    }
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机
  * @retval         none
  */
static void gimbal_motor_absolute_yaw_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }

    // 角度环,速度环串级pid调试
    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_absolute_angle_pid, gimbal_motor->absolute_angle, gimbal_motor->absolute_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_absolute_gyro_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);
    // 控制值赋值
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_GYRO，使用陀螺仪计算的欧拉角进行控制
  * @author         吴森桦
  * @param          gimbal_motor pitch电机
  * @retval         none
  */
static void gimbal_motor_absolute_pitch_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }

    // 角度环,速度环串级pid调试
    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_absolute_angle_pid, gimbal_motor->absolute_angle, gimbal_motor->absolute_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_absolute_gyro_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);		
    // 控制值赋值
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机或者pitch电机
  * @retval         none
  */
static void gimbal_motor_relative_yaw_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }

    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_relative_angle_pid, gimbal_motor->relative_angle, gimbal_motor->relative_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_relative_enconde_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

/**
  * @brief          云台控制模式 GIMBAL_MOTOR_ENCONDE，使用编码相对角进行控制
  * @author         吴森桦
  * @param          gimbal_motor yaw电机或者pitch电机
  * @retval         none
  */
static void gimbal_motor_relative_pitch_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }

    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_relative_angle_pid, gimbal_motor->relative_angle, gimbal_motor->relative_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_relative_enconde_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

static void gimbal_motor_CV_yaw_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }
    // ??????????????pid???? ,??????pid????
    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_CV_angle_pid, gimbal_motor->absolute_angle,gimbal_motor->absolute_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_CV_gyro_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

static void gimbal_motor_CV_pitch_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }
    // ??????????????pid????
    gimbal_motor->motor_gyro_set = gimbal_PID_calc(&gimbal_motor->gimbal_motor_CV_angle_pid, gimbal_motor->relative_angle, gimbal_motor->relative_angle_set, gimbal_motor->motor_gyro);
    gimbal_motor->current_set = PID_calc(&gimbal_motor->gimbal_motor_CV_gyro_speed_pid, gimbal_motor->motor_gyro, gimbal_motor->motor_gyro_set);
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}


/**
  * @brief          云台控制模式 GIMBAL_MOTOR_RAW，电流值直接发送到CAN总线
  * @author         吴森桦
  * @param          gimbal_motor yaw电机或者pitch电机
  * @retval         none
  */
static void gimbal_motor_raw_angle_control(gimbal_motor_t *gimbal_motor)
{
    if (gimbal_motor == NULL)
    {
        return;
    }
    gimbal_motor->current_set = gimbal_motor->raw_cmd_current;
    gimbal_motor->given_current = (int16_t)(gimbal_motor->current_set);
}

/**
  * @brief          云台角度PID初始化, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:云台PID指针
  * @param[in]      maxout: pid最大输出
  * @param[in]      intergral_limit: pid最大积分输出
  * @param[in]      kp: pid kp
  * @param[in]      ki: pid ki
  * @param[in]      kd: pid kd
  * @retval         none
  */
static void gimbal_PID_init(gimbal_PID_t *pid, fp32 maxout, fp32 max_iout, fp32 kp, fp32 ki, fp32 kd)
{
    if (pid == NULL)
    {
        return;
    }
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;

    pid->err = 0.0f;
    pid->get = 0.0f;
    pid->max_iout = max_iout;
    pid->max_out = maxout;
}

/**
  * @brief          云台角度PID计算, 因为角度范围在(-pi,pi)，不能用PID.c的PID
  * @param[out]     pid:云台PID指针
  * @param[in]      get: 角度反馈
  * @param[in]      set: 角度设定
  * @param[in]      error_delta: 角速度
  * @retval         pid 输出
  */
static fp32 gimbal_PID_calc(gimbal_PID_t *pid, fp32 get, fp32 set, fp32 error_delta)
{
    fp32 err;
    if (pid == NULL)
    {
        return 0.0f;
    }
    pid->get = get;
    pid->set = set;

    err = set - get;
    pid->err = rad_format(err);
    pid->Pout = pid->kp * pid->err;
    pid->Iout += pid->ki * pid->err;
    pid->Dout = pid->kd * error_delta;
    abs_limit(&pid->Iout, pid->max_iout);
    pid->out = pid->Pout + pid->Iout + pid->Dout;
    abs_limit(&pid->out, pid->max_out);
    return pid->out;
}

/**
  * @brief          云台PID清除，清除pid的out,iout
  * @param[out]     pid_clear:"gimbal_control"变量指针.
  * @retval         none
  */
static void gimbal_PID_clear(gimbal_PID_t *gimbal_pid_clear)
{
    if (gimbal_pid_clear == NULL)
    {
        return;
    }
    gimbal_pid_clear->err = gimbal_pid_clear->set = gimbal_pid_clear->get = 0.0f;
    gimbal_pid_clear->out = gimbal_pid_clear->Pout = gimbal_pid_clear->Iout = gimbal_pid_clear->Dout = 0.0f;
}
